import os, sys
print(os.path.dirname(os.getcwd()))
sys.path.append(os.path.dirname(os.getcwd()))
from car.car_serial import CarSerial
from car.car_pca9685 import PCA9685
from car.car_timer import CarTimer
from configobj import ConfigObj


class Motor:
    def __init__(self):
        self.conf = ConfigObj("./setting.ini", encoding='utf8')
        self.drive_board = None
        self.timer = CarTimer(interval=0.1)
        self.load_setting()

    def load_setting(self):
        para = self.conf['car_controller']
        driver_type = int(para['type'])
        if driver_type == 0:
            if int(para['port']) == 0:
                port = "/dev/ttyACM0"
            else:
                port = "/dev/ttyUSB0"
            rate = int(para['rate'])
            if para['receive_arduino'] == "True":
                is_receive = True
            else:
                is_receive = False
            self.drive_board = CarSerial(port=port, baud_rate=rate, receive=is_receive)
        else:
            address = int(para['iic_address'], base=16)
            freq = int(para['frequency'])
            if para['receive_pca'] == "True":
                is_receive = True
            else:
                is_receive = False
            board_type = int(para['board_type'])
            self.drive_board = PCA9685(address=address, frequency=freq, debug=is_receive, board_type=board_type)

    def drive_motor(self, left_speed, right_speed):
        if self.timer.timeout():
            self.drive_board.drive_motor(left_speed, right_speed)
            print("left:{0},right:{1}".format(left_speed,right_speed))
            self.timer.restart()

    def stop(self):
        self.drive_board.drive_motor(0, 0)
